/*
 * Plan.cpp
 *
 *  Created on: 14 Feb 2012
 *      Author: mohab
 */

#include "Enums.h"
#include "Plan.h"
#include "Server.h"
#include "string"
#include <pthread.h>
#include "Agent.h"
#include <sstream>
#include "Client.h"
#include <iostream>

using namespace std;
inline Plan::Plan(World world, Agent* agents, int agentCount) {
	this->world = world;
	this->pck_counter = 0;//replace with 1,2 on the other agents
	/*
	 * pck_coutner is used to keep track
	 * of the pucks and number them differently
	 * on the three agents, this method shall
	 * be replaced later on with a better one
	 */
	this->agentCount = agentCount;
	this->agents = agents;
	agent = agents; //setting the server's agent variable
	m1Discovered=0;
	m2Discovered=0;
	m3Discovered=0;
	this->agents->m1Discovered = &m1Discovered;
	this->agents->m2Discovered = &m2Discovered;
	this->agents->m3Discovered = &m3Discovered;
	printf("%d",*this->agents->m1Discovered);
	this->discovery = true;
	(*agents).world = &world;
	//	agents[1].world = &world;
	//	agents[2].world = &world;
	this->machineDiscovered = true;
	this->noDiscoveredMachines = -1;
	//server thread
	//pthread_t thread_id;
	//pthread_create(&thread_id, NULL, &ServerInit, (void*) agents);
	//pthread_detach(thread_id);
//	while (true)
//		;
}
bool Plan::nextStep() {
	if (discovery) {
		if (machineDiscovered) {
			noDiscoveredMachines++;

			if (noDiscoveredMachines < sizeof(DISCOVERY_PRIORITY1))//1,2,3
			{
				machineToDiscover = DISCOVERY_PRIORITY1[noDiscoveredMachines]; //change to 2,3 on plan2,3
				machineDiscovered = false;
			} else {
				discovery = false;
			}
		} else
			Discover(machineToDiscover);
		// Target number of machines to be discovered and their types
		// Communicate
		if (m1Discovered >= 2 && m2Discovered >= 2 && m3Discovered >= 2)
			discovery = false;

		return true;

	} else {
		PostDiscovery();
		return false;
	}

	return false;
}

void Plan::PostDiscovery() {
	//communicate to synch


	/*
	 * FEED = Insert raw puck to machine
	 * PICK = Move produced puck from a
	 *        machine and Insert in another machine
	 *
	 * 7 Steps to create a product:
	 * ============================
	 *
	 * STP_1 - FEED S0 M1
	 * STP_2 - PICK S1 FROM M1 TO M2
	 * STP_3 - FEED S0 M2
	 * STP_4 - PICK S2 M2 TO M3
	 * STP_5 - FEED S0 TO M1
	 * STP_6 - FEED S0 TO M3
	 * STP_7 - PICK S1 FROM M1 TO M3
	 */

	// QUESTION: What to do about consumed pucks??

	// Build command depending on the Step
	stringstream s;
	int chosenM1, chosenM2, chosenM3, chosenDG;
	int s1_index, s2_index, s3_index, s4_index, s5_index, s6_index, s7_index;
	//UPDATE STUFF INSIDE HERE!!!
	if (!STP_1) {
		chosenM1 = getClosestMachine(Enums::M1);
		s1_index = getClosestPuck(PCK_S0);
		s << CMD_FEED + "," << chosenM1 + "," << PCK_S0 + "," << "," + s1_index;
		STP_1 = true;
	} else if (!STP_2) {
		chosenM2 = getClosestMachine(Enums::M2);//get closest M1?
		s2_index = getClosestPuck(PCK_S1);
		//WHICH PUCK IS BEING PICKED???
		s << CMD_PICK + "," << chosenM2 + "," << PCK_S1 + "," << "," + chosenM1;
		//MY VERSION:s << CMD_PICK + "," << chosenM2 + "," << getClosestPuck(PCK_S1) + "," << "," + chosenM1;
		STP_2 = true;
	} else if (!STP_3) {
		s << CMD_FEED + "," << chosenM2 + "," << PCK_S0 + "," << ","
				+ getClosestPuck(PCK_S0);
		s3_index = getClosestPuck(PCK_S0);
		STP_3 = true;
	} else if (!STP_4) {
		chosenM3 = getClosestMachine(Enums::M3);
		s << CMD_PICK + "," << chosenM3 + "," << PCK_S2 + "," << "," + chosenM2;
		s4_index = getClosestPuck(PCK_S1);
		//MY VERSION:s << CMD_PICK + "," << chosenM2 + "," << getClosestPuck(PCK_S2) + "," << "," + chosenM1;
		STP_4 = true;
	} else if (!STP_5) {
		chosenM1 = getClosestMachine(Enums::M1);
		s << CMD_FEED + "," << chosenM1 + "," << PCK_S0 + "," << ","
				+ getClosestPuck(PCK_S0);
		s5_index = getClosestPuck(PCK_S1);
		STP_5 = true;
	} else if (!STP_6) {
		s << CMD_FEED + "," << chosenM3 + "," << PCK_S0 + "," << ","
				+ getClosestPuck(PCK_S0);
		s6_index = getClosestPuck(PCK_S1);
		STP_6 = true;
	} else if (!STP_7) {
		s << CMD_PICK + "," << chosenM3 + "," << PCK_S1 + "," << "," + chosenM1;
		//MY VERSION:s << CMD_PICK + "," << chosenM2 + "," << getClosestPuck(PCK_S1) + "," << "," + chosenM1;
		s7_index = getClosestPuck(PCK_S1);
		STP_7 = true;
	} else {
		// TODO: Feed the delivery gate from chosenM3
		chosenDG = getWorkingDG();
		//MY VERSION:s << CMD_PICK + "," << chosenDG + "," << getClosestPuck(PCK_P) + "," << "," + chosenM3;
		STP_1 = STP_2 = STP_3 = STP_4 = STP_5 = STP_6 = STP_7 = false; // Repeat all over again
	}

	// Send command to agent
	agents[0].interpretCommand(s.str().c_str(), new bool(false));

	/*
	 * EXPRESS-GOOD ROBOT PSEUDO.
	 * wait for express good
	 * goto M1
	 * deliver express product to DG
	 * goto back and wait for express good
	 */

}

// Gets the distance from a location
int getDistance(int loc) {
	// Call Motion planning code here
	return 0;
}

int Plan::getClosestPuck(int type) {
	int closestIndex = -1;
	int closestDistance = 1000000;
	for (int i = 1; i < 20; i++) {
		if (world.pucks[i].state == type) {
			int tmp = getDistance(world.pucks[i].location);
			if (tmp < closestDistance) {
				closestDistance = tmp;
				closestIndex = i;
			}
		}
	}
	return closestIndex;
}
int Plan::getWorkingDG() {
	/*
	 * use vision or whatever to determine the working DG
	 */
	return LOC_DG1;
}
int Plan::getClosestMachine(Enums::MachineType type) {
	int * availableMachinesOfType = new int[5]; // set to hold 5 machines which is the max if machine type is M1

	int currentIndex;
	/*
	 * Why did we check if the type is Enums::M1?
	 * if(type==Enums::M1)
	 * {
	 */
	for (int i = 0; i < 13; i++) {
		if (world.machines[i].type == type && world.machines[i].state
				== Enums::On) {
			availableMachinesOfType[currentIndex] = world.machines[i].number;
			currentIndex++;
		}
	}
	//Call Motion Planning to get Closest machine
	/*
	 * }
	 */
	return 0;// TODO: change later with the closest machine returned by the motion planning code
}

void Plan::Discover(int machineNo) {
	// Find closest Agent

	int distance = 10000000;
	Agent closestAgent = Agent();
	/*for(int i = 0; i < agentCount; i++)
	 {
	 int tmpdist = agents[i].getDistanceToMachine(machineNo);//distance to loading_area+distance_to_machine
	 if( tmpdist < distance)
	 {
	 distance = tmpdist;
	 closestAgent = agents[i];
	 }
	 }
	 will be used later when the plan is more decentrailized
	 */
	closestAgent = agents[0];
	// Send command to discover and Repeat until Product produced
	bool * produced = false;
	Enums e = Enums();
	stringstream s;
	//are the cmds correct?
	//why s1 one 4th param containing location, while in s2 and s3 contains puck index?
	//added  5th param, the puck index!!!
	s << CMD_DISCOVER + "," << machineNo + "," << PCK_S0 + ","
			<< LOC_LOADING_AREA + "," + this->pck_counter;//13=loading area
	const char * s1 = s.str().c_str();
	s.clear();
	int s2_index = getClosestPuck(PCK_S1);
	s << CMD_DISCOVER + "," << machineNo + "," << PCK_S1 + ","
			<< (this->world.pucks[s2_index].location) + "," + s2_index;
	const char * s2 = s.str().c_str();
	s.clear();
	int s3_index = getClosestPuck(PCK_S2);
	s << CMD_DISCOVER + "," << machineNo + "," << PCK_S2 + ","
			<< (this->world.pucks[s3_index].location) + "," + s3_index;
	const char * s3 = s.str().c_str();
	s.clear();
	//we dont keep track of which puck is inserted into which machine to update the types
	while (!produced) {
		stringstream s;
		if (world.machines[machineNo].type == e.Unknown0) {
			closestAgent.interpretCommand(s1, produced);
			if (!produced) {
				world.machines[machineNo].type = e.Unknown1;
				s << CMD_UPD_MTP + "," << machineNo + "," << 1;
				closestAgent.communicate(s.str().c_str());
				s.clear();
				this->world.pucks[this->pck_counter].state = PCK_R; //if nothing produced then this puck is waste and needs recycling
				s << CMD_UPD_PST + "," << this->pck_counter + "," << PCK_R;
				closestAgent.communicate(s.str().c_str());
				s.clear();
				//this->world.pucks[this->pck_counter].location = machineNo;
				//location is useless, updated in agent
				this->pck_counter += 3;
			} else {
				world.machines[machineNo].type = e.M1;
				s << CMD_UPD_MTP + "," << machineNo + "," << 2; //2 represents e.M1
				closestAgent.communicate(s.str().c_str());
				s.clear();
				m1Discovered++;
				s << CMD_UPD_MDS + "," << 2 + "," << m1Discovered; //2 represents M1
				closestAgent.communicate(s.str().c_str());
				s.clear();
				this->world.pucks[this->pck_counter].state = PCK_S1;
				s << CMD_UPD_PST + "," << this->pck_counter + "," << PCK_S1;
				closestAgent.communicate(s.str().c_str());
				s.clear();
				//this->world.pucks[this->pck_counter].location = machineNo;
				this->pck_counter += 3;
			}
		} else if (world.machines[machineNo].type == e.Unknown1) {
			closestAgent.interpretCommand(s2, produced);
			if (!produced) {
				world.machines[machineNo].type = e.M3;
				s << CMD_UPD_MTP + "," << machineNo + "," << 4;
				closestAgent.communicate(s.str().c_str());
				s.clear();
				//	this->world.pucks[this->pck_counter].location = machineNo;
				this->world.pucks[s2_index].state = PCK_R;
				s << CMD_UPD_PST + "," << s2_index + "," << PCK_R;
				closestAgent.communicate(s.str().c_str());
				s.clear();
			} else {
				world.machines[machineNo].type = e.M2;
				s << CMD_UPD_MTP + "," << machineNo + "," << 3;
				closestAgent.communicate(s.str().c_str());
				s.clear();
				//this->world.pucks[this->pck_counter].location = machineNo;
				m2Discovered++;
				s << CMD_UPD_MDS + "," << 3 + "," << m2Discovered;
				closestAgent.communicate(s.str().c_str());
				s.clear();
				this->world.pucks[s2_index].state = PCK_S2;
				s << CMD_UPD_PST + "," << s2_index + "," << PCK_S2;
				closestAgent.communicate(s.str().c_str());
				s.clear();
			}
		} else if (world.machines[machineNo].type == e.M3) {
			closestAgent.interpretCommand(s3, produced);
			if (produced) {
				m3Discovered++;
				s << CMD_UPD_MDS + "," << 4 + "," << m3Discovered;
				closestAgent.communicate(s.str().c_str());
				s.clear();
				this->world.pucks[s3_index].state = PCK_P;
				s << CMD_UPD_PST + "," << s3_index + "," << PCK_P;
				closestAgent.communicate(s.str().c_str());
				s.clear();
			}
		}
	}
}
int main(int a, char **arg) {
	/*Agent agent = Agent();
	 agent.interpretCommunicationCommand("8,2,3");
	 while(true);*/

	Agent agent = Agent();
	World world = World();
	world.Init();
	Plan plan = Plan(world, &agent, 1);
}
